/*
 * Copyright (c) 2006-2021, RT-Thread Development Team
 *
 * SPDX-License-Identifier: Apache-2.0
 *
 * Change Logs:
 * Date           Author       Notes
 * 2021-07-15     MBW       the first version
 */
#include "pid2.h"
#include "maths.h"
#include "filter.h"
#include "JY901B_931.h"
//#define   EnIIRDeiv
#define delta_pwm 2

PID_param PID_PAR={0};
biquadFilter_t StaDeivFilterLPF[3];// 二阶低通滤波器
float cascade_pid_pitch_test(float eur,float mpu_eur,float mpu_gyro
                            ,float akp,float aki,float akd
                            ,float rkp,float rki,float rkd)
{
    /*pid误差变量*/
    static float a_err1,a_err2,r_err1,r_err2;//用于积分项的误差累积
    float a_err0,r_err0,output_pwm,out_rate,deiv;
    //角度环PID
    a_err0 = eur-mpu_eur;
    out_rate=akp*a_err0 + aki*(a_err0+a_err1+a_err2) + akd*(a_err0-a_err1);
    a_err1 = a_err0; a_err2 = a_err1;
    //角速度环PID
    r_err0 = out_rate-mpu_gyro;  // r_err0 = out_rate-mpu_gyro*RAD2DEG;
    deiv = r_err0-r_err1;
//#ifdef EnIIRDeiv
//     deiv  =   biquadFilterApply(&StaDeivFilterLPF[0], deiv);
//#endif
    output_pwm=rkp*r_err0 + rki*(r_err0+r_err1+r_err2) + rkd*deiv;
    r_err1 = r_err0; r_err2 = r_err1;
    //返回结果
    return output_pwm;
}
float cascade_pid_roll_test(float eur,float mpu_eur,float mpu_gyro
                            ,float akp,float aki,float akd
                            ,float rkp,float rki,float rkd)
{
    /*pid误差变量*/
    static float a_err1,a_err2,r_err1,r_err2;//用于积分项的误差累积
    float a_err0,r_err0,output_pwm,out_rate,deiv;
    //角度环PID
    a_err0 = eur-mpu_eur;
    out_rate=akp*a_err0 + aki*(a_err0+a_err1+a_err2) + akd*(a_err0-a_err1);
    a_err1 = a_err0; a_err2 = a_err1;
    //角速度环PID
    r_err0 = out_rate-mpu_gyro;  // r_err0 = out_rate-mpu_gyro*RAD2DEG;
    deiv = r_err0-r_err1;
//#ifdef EnIIRDeiv
//     deiv  =   biquadFilterApply(&StaDeivFilterLPF[1], deiv);
//#endif
    output_pwm=rkp*r_err0 + rki*(r_err0+r_err1+r_err2) + rkd*deiv;
    r_err1 = r_err0; r_err2 = r_err1;
    //返回结果
    return output_pwm;
}
float cascade_pid_yaw_test(float eur,float mpu_eur,float mpu_gyro
                            ,float akp,float aki,float akd
                            ,float rkp,float rki,float rkd)
{
    /*pid误差变量*/
    static float a_err1,a_err2,r_err1,r_err2;//用于积分项的误差累积
    float a_err0,r_err0,output_pwm,out_rate,deiv;
    //角度环PID
    a_err0 = eur-mpu_eur;
    out_rate=akp*a_err0 + aki*(a_err0+a_err1+a_err2) + akd*(a_err0-a_err1);
    a_err1 = a_err0; a_err2 = a_err1;
    //角速度环PID
    r_err0 = out_rate-mpu_gyro;  // r_err0 = out_rate-mpu_gyro*RAD2DEG;
    deiv = r_err0-r_err1;
//#ifdef EnIIRDeiv
//     deiv  =   biquadFilterApply(&StaDeivFilterLPF[2], deiv);
//#endif
    output_pwm=rkp*r_err0 + rki*(r_err0+r_err1+r_err2) + rkd*deiv;
    r_err1 = r_err0; r_err2 = r_err1;
    //返回结果
    return output_pwm;
}
float vel_pid_x_test(float set_vel ,float nav_vel
                    ,float vel_k,float vel_i,float vel_d)
{
    static float vel_err1,vel_err2 ;
    float vel_err ;
    float output_pwm;
//    if(nav_vel<0)nav_vel=nav_vel*0.8;
    //速度环PID
    vel_err = set_vel-nav_vel;
    output_pwm = vel_k*vel_err + vel_i*(vel_err+vel_err1+vel_err2) + vel_d*(vel_err-vel_err1);
    vel_err1 = vel_err;vel_err2 = vel_err1;
    //返回结果
    return output_pwm;
}
float vel_pid_y_test(float set_vel ,float nav_vel
                            ,float vel_k,float vel_i,float vel_d)
{
    static float vel_err1,vel_err2 ;
    float vel_err ;
    float output_pwm;
    //速度环PID
    vel_err = set_vel-nav_vel;
    output_pwm = vel_k*vel_err + vel_i*(vel_err+vel_err1+vel_err2) + vel_d*(vel_err-vel_err1);
    vel_err1 = vel_err;vel_err2 = vel_err1;
    //返回结果
    return output_pwm;
}
float vel_pid_z_test(float set_vel ,float nav_vel
                            ,float vel_k,float vel_i,float vel_d)
{
    static float vel_err1,vel_err2 ;
    float vel_err ;
    float output_pwm;
    //速度环PID
    vel_err = set_vel-nav_vel;
    output_pwm = vel_k*vel_err + vel_i*(vel_err+vel_err1+vel_err2) + vel_d*(vel_err-vel_err1);
    vel_err1 = vel_err;vel_err2 = vel_err1;
    //返回结果
    return output_pwm;
}
float cas_height_pid_test(float set_h ,float nav_h,float nav_vz
                         ,float limit_vel_up,float limit_vel_down
                         ,float hp , float hi ,float hd
                         ,float vzp ,float vzi,float vzd )
{
    static float h_err1,h_err2 , vz_err1,vz_err2;
    float  h_err , vz_err ;
    float  out_1 , out_2 , out_thrust;
    //高度环PID
    h_err = set_h-nav_h;//当大于设定高度时，out_1是负数 ，反之为+
    out_1 = hp*h_err + hi*(h_err+h_err1+h_err2) + hd*(h_err-h_err1);
    h_err1 = h_err ; h_err2 = h_err1;
    //油门速度PID
  //  vz_err = out_1-nav_vz;//下降速度是-数
    out_1 = constrainf(out_1, limit_vel_down, limit_vel_up);
    vz_err = out_1-nav_vz;
    out_2 = vzp*vz_err + vzi*(vz_err+vz_err1+vz_err2) + vzd*(vz_err-vz_err1);
    vz_err1 = vz_err ; vz_err2 = vz_err1;
     out_thrust = out_2;
    //返回结果
    return out_thrust;
}
float k210_sevrox_pid(float pos_set ,float pos_get
                    ,float pos_k,float pos_i,float pos_d,u8 sta)
{
    static float pos_err1,pos_err2 ;
    float pos_err ;
    float output_pwm;
    if(sta==1)pos_err=0,pos_err1=0,pos_err2=0;
//    if(nav_vel<0)nav_vel=nav_vel*0.8;
    //速度环PID
    pos_err = pos_set-pos_get;
    output_pwm = pos_k*pos_err + pos_i*(pos_err+pos_err1+pos_err2) + pos_d*(pos_err-pos_err1);
    pos_err1 = pos_err;pos_err2 = pos_err1;
    //返回结果
    return output_pwm;
}
float k210_sevroy_pid(float pos_set ,float pos_get
                    ,float pos_k,float pos_i,float pos_d,u8 sta)
{
    static float pos_err1,pos_err2 ;
    float pos_err ;
    float output_pwm;
    if(sta==1)pos_err=0,pos_err1=0,pos_err2=0;
//    if(nav_vel<0)nav_vel=nav_vel*0.8;
    //速度环PID
    pos_err = pos_set-pos_get;
    output_pwm = pos_k*pos_err + pos_i*(pos_err+pos_err1+pos_err2) + pos_d*(pos_err-pos_err1);
    pos_err1 = pos_err;pos_err2 = pos_err1;
    //返回结果
    return output_pwm;
}
float cas_velx_pid_test(float target ,float get_data1,float get_data2
                         ,float limit_vel_up,float limit_vel_down
                         ,float kp1 , float ki1 ,float kd1
                         ,float kp2 , float ki2, float kd2 )
{
    static float pid1_err1,pid1_err2 , pid2_err1,pid2_err2 ;
    float  pid1_err , pid2_err ;
    float  pid1_out , pid2_out ;
    //主控制器PID1
    pid1_err = target-get_data1;//当大于设定高度时，out_1是负数 ，反之为+
    pid1_out = kp1*pid1_err + ki1*(pid1_err+pid1_err1+pid1_err2) + kd1*(pid1_err-pid1_err1);
    pid1_err1 = pid1_err ; pid1_err2 = pid1_err1;
    pid1_out = constrainf(pid1_out, -limit_vel_down, +limit_vel_up);
    //副控制器PID2
    pid2_err = pid1_out - get_data2;
    pid2_out = kp2*pid2_err + ki2*(pid2_err+pid2_err1+pid2_err2) + kd2*(pid2_err-pid2_err1);
    pid2_err1 = pid2_err ; pid2_err2 = pid2_err1;
    //输出限幅
 //   pid2_out = constrainf(pid2_out,-30,+30);
    //返回结果
    return pid2_out;
}
float cas_vely_pid_test(float target ,float get_data1,float get_data2
        ,float limit_vel_up,float limit_vel_down
        ,float kp1 , float ki1 ,float kd1
        ,float kp2 , float ki2, float kd2 )
{
    static float pid1_err1,pid1_err2 , pid2_err1,pid2_err2 ;
    float  pid1_err , pid2_err ;
    float  pid1_out , pid2_out ;
    //主控制器PID1
    pid1_err = target-get_data1;//当大于设定高度时，out_1是负数 ，反之为+
    pid1_out = kp1*pid1_err + ki1*(pid1_err+pid1_err1+pid1_err2) + kd1*(pid1_err-pid1_err1);
    pid1_err1 = pid1_err ; pid1_err2 = pid1_err1;
    pid1_out = constrainf(pid1_out, -limit_vel_down, +limit_vel_up);
    //副控制器PID2
    pid2_err = pid1_out - get_data2;
    pid2_out = kp2*pid2_err + ki2*(pid2_err+pid2_err1+pid2_err2) + kd2*(pid2_err-pid2_err1);
    pid2_err1 = pid2_err ; pid2_err2 = pid2_err1;
    //输出限幅
//    pid2_out = constrainf(pid2_out,-30,+30);
    //返回结果
    return pid2_out;
}
float pid_dis_z(float target , float get_data , float kp , float ki , float kd)
{
    static float err1 , err2 ;
    float err , out ;
    err = target  - get_data ;
    out = kp*err + ki * (err+err1+err2) + kd * (err-err1);
    err1 = err;  err2 = err1;
    return out;
}
float pid_vel_z(float target , float get_data , float kp , float ki , float kd)
{
    static float err1 , err2 ;
    float err , out ;
    err = target  - get_data ;
    out = kp*err + ki * (err+err1+err2) + kd * (err-err1);
    return out;
}
float pid_testx(float target , float get_data , float kp , float ki , float kd)
{
    static float err1 , err2 ;
    float err , out ;
    err = target  - get_data ;
    out = kp*err + ki * (err+err1+err2) + kd * (err-err1);
    return out;
}
float pid_testy(float target , float get_data , float kp , float ki , float kd)
{
    static float err1 , err2 ;
    float err , out ;
    err = target  - get_data ;
    out = kp*err + ki * (err+err1+err2) + kd * (err-err1);
    return out;
}
float dis_pid_x_test(float set_vel ,float nav_vel
                    ,float vel_k,float vel_i,float vel_d)
{
    static float vel_err1,vel_err2 ;
    float vel_err ;
    float output_pwm;
//    if(nav_vel<0)nav_vel=nav_vel*0.8;
    //速度环PID
    vel_err = set_vel-nav_vel;
    output_pwm = vel_k*vel_err + vel_i*(vel_err+vel_err1+vel_err2) + vel_d*(vel_err-vel_err1);
    vel_err1 = vel_err;vel_err2 = vel_err1;
    //返回结果
    return output_pwm;
}
float dis_pid_y_test(float set_vel ,float nav_vel
                            ,float vel_k,float vel_i,float vel_d)
{
    static float vel_err1,vel_err2 ;
    float vel_err ;
    float output_pwm;
    //速度环PID
    vel_err = set_vel-nav_vel;
    output_pwm = vel_k*vel_err + vel_i*(vel_err+vel_err1+vel_err2) + vel_d*(vel_err-vel_err1);
    vel_err1 = vel_err;vel_err2 = vel_err1;
    //返回结果
    return output_pwm;
}
float dev_pid_test1(float target,float get_data,float kp,float ki,float kd,float g)
{
    static float err1,err2,get_data_last,dev_last;
    float c1,c2,c3,temp,dev=0,err ,out;
    err=target-get_data;
    temp = g*kp + kd;
    c3 = kd/temp;
    c2 = (kd+kp)/temp;
    c1 = g*c3;
    dev = c1*dev_last+c2*get_data+c3*get_data_last;
    out = kp*err+ki*(err+err1+err2)+dev;
    err1 = err;err2 =err1;
    get_data_last = get_data;dev_last = dev;
    return out;
}

void StaDeivFilterLPF_init(void)
{
    //初始化二阶低通滤波
    for (int axis = 0; axis < 3; axis++)//con_fre Hz
    {
        biquadFilterInitLPF(&StaDeivFilterLPF[axis],  200, 100);
    }
    rt_kprintf("StaDeivFilterLPF_init is ok! \n");
}
INIT_APP_EXPORT(StaDeivFilterLPF_init);
